/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

//import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;

public class Team3559Robot extends SimpleRobot {
    RobotDrive drive = new RobotDrive (1,2); //Drive  
    Joystick LeftStick = new Joystick(1);//left drive stick
    Joystick RightStick = new Joystick (2);//right drive stick
    Joystick ThirdStick = new Joystick (3);//arm joystick
    Solenoid climb = new Solenoid(1);
    Relay climb2 = new Relay(1);
    double autoalignspeed = .6;
    
    int locked;
    double dist;
    int target;
    int loaderstat;
    boolean override;
    boolean autotrigger;
    Relay Compressor = new Relay (2);
    Servo Servo2 = new Servo(7);
    boolean drivemode;
    Talon shooter1 = new Talon(3);
    Talon shooter2 = new Talon(4);
    Talon tilt1 = new Talon(9);
    Talon tilt2 = new Talon(10);
    // pot = new Analog(1);
    Victor loader = new Victor(5);
    Servo servo = new Servo(8);
    double kinectangle;
    double kinectDistance;
    double loaderspeed;
    double shooterspeed;
    boolean setup;
    boolean lockedmiddle;
    boolean lockedtop;
    double autodistance1;
    double autodistance2;
    DigitalInput indexlimit = new DigitalInput(2);
    DigitalInput loaderlimit = new DigitalInput(1);
   // double[] tiltAngleArray = {2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.92,2.919,2.919,2.919,2.919,2.919,2.919,2.919,2.9,2.89,2.5,2.837,2.837,2.837,2.837,2.837,2.837,2.837,2.837,2.837,2.837,2.837};
    int index1 = (int)kinectDistance;
   // double tiltAngle = tiltAngleArray[index1];
    //Ultrasonic ultra = new Ultrasonic(ping,echo);
    AnalogChannel ultra = new AnalogChannel(2);
    AnalogChannel pot = new AnalogChannel(1);
    int autotimes;
    public void autodistance(){
    if (kinectDistance >= 20.25){
        drive.tankDrive(autoalignspeed, autoalignspeed);
    }
    if (kinectDistance <= 19.75){
        drive.tankDrive(-autoalignspeed, -autoalignspeed);
    }
//    if ((kinectDistance > 20) & (kinectDistance < 19.5)){
////            if (kinectDistance >= ( ){
////        drive.tankDrive(.35, .35);
////    }
////    if (kinectDistance <= (autodistance2 + .37)){
////        drive.tankDrive(-.35, -.35);
////    }
    System.out.println("SECOND STAGE DISTANCE");
    if (((kinectDistance > 19.5)) & ((kinectDistance < 20))){
        drive.tankDrive(0.0, 0.0);
    }
    }
    public void autoalign(){
//   if (kinectDistance <= 10){
//        drive.tankDrive(-1.0, -1.0);
//    } if (kinectDistance <= 10){
//        drive.tankDrive(-1.0, -1.0);
//    }
    
        // System.out.println(tiltAngle);
        System.out.println("in autoalign");
        //autoalignspeed = (((LeftStick.getZ(GenericHID.Hand.kLeft)+1))/2);
        
    if (kinectangle <=-1){
        drive.tankDrive(autoalignspeed, -autoalignspeed);
    }
    if (kinectangle >= 1){
        drive.tankDrive(-autoalignspeed, autoalignspeed);
    }
    if ((kinectangle < 1) & (kinectangle > -1)){
//        if (kinectangle <=-.25){
        drive.tankDrive(0.0, 0.0);
//    }
//    if (kinectangle >= .25){
//        drive.tankDrive(0.0, .35);
//    }
//    }
//    if ((kinectangle < .25) & (kinectangle > -.25)){
//        drive.tankDrive(0.0, 0.0);
//    System.out.println("Made it Thru");
    //TILT TRACKING
//    if (potentiometer.getVoltage() <= (tiltAngle - .1)){
//        tilt1.set(autoalignspeed);
//        tilt2.set(autoalignspeed);
//    }
//    if (potentiometer.getVoltage() >= (tiltAngle + .1)){
//        tilt1.set(autoalignspeed);
//        tilt2.set(autoalignspeed);
//    }
//    if ((potentiometer.getVoltage() >= (tiltAngle - .1)) & (potentiometer.getVoltage() <= (tiltAngle + .1))){
//        tilt1.set(0.0);
//        tilt2.set(0.0);
//    }
    
    }
}
    public void autoadjust(){
    if (LeftStick.getRawButton(9)== true){
        if ((kinectangle >=1) || (kinectangle <=-1)){
            autoalign();
        }else{
            autodistance();
        }
    }
    }
    public void teleloader(){
    if (ThirdStick.getRawButton(8) == true){
        loaderstat = 1;
    }
    if (loaderstat == 1){
        loader.set(-1.0);
        if (loaderlimit.get() == true){
            loaderstat = 2;
    }
    }
    if(loaderstat == 2){
        if (loaderlimit.get() == false){
            loader.set(0.0);
            loaderstat = 0;
            
        }
    }
    }
    public void autoloader(){
    if (ThirdStick.getRawButton(8) == true){
        loaderstat = 1;
    }
    if (loaderstat == 1){
        loader.set(-1.0);
        if (loaderlimit.get() == true){
            loaderstat = 2;
    }
    }
    if(loaderstat == 2){
        if (loaderlimit.get() == false){
            loader.set(0.0);
            loaderstat = 0;
            
        }
    }
    }
    public void update(){
        //smart dashboard update
        //SmartDashboard.putNumber("Pot", pot);
        SmartDashboard.putNumber("ThirdStick Z axis", (((ThirdStick.getZ(GenericHID.Hand.kLeft)+1))/2));
        SmartDashboard.putNumber("RightStick Z axis", (((RightStick.getZ(GenericHID.Hand.kLeft)+1))/2));
        SmartDashboard.putNumber("LeftStick Y Axis", -(LeftStick.getY(GenericHID.Hand.kLeft)));
        SmartDashboard.putNumber("RightStick Y Axis", -(RightStick.getY(GenericHID.Hand.kLeft)));
        SmartDashboard.putNumber("LeftStick X Axis", (LeftStick.getX(GenericHID.Hand.kLeft)));
        SmartDashboard.putNumber("RightStick X Axis", (RightStick.getX(GenericHID.Hand.kLeft)));
        SmartDashboard.putNumber("KINECTDISTANCE", kinectDistance);
        SmartDashboard.putNumber("ultra", ultra.getValue());
        SmartDashboard.putNumber("angle of shooter", ((pot.getVoltage()) - 2.1675)/0.0271);
        SmartDashboard.putNumber("pot", pot.getValue());
        SmartDashboard.putNumber("potentiometer", pot.getVoltage());
        SmartDashboard.putNumber("Target", target);
        SmartDashboard.putBoolean("Middle Goal", lockedmiddle);
        SmartDashboard.putBoolean("Top Goal", lockedtop);
        SmartDashboard.putNumber("Servo Angle", Servo2.getAngle());
        SmartDashboard.putBoolean("Trigger Switch", loaderlimit.get());
        SmartDashboard.putBoolean("Override", override);
    }
    
    public void tracking(){
	NetworkTable server = NetworkTable.getTable("SmartDashboard");
	try
	{
	    System.out.println(server.getNumber("IMAGE_COUNT", 0.0));	
            System.out.println(server.getNumber("DISTANCE"));
            System.out.println(server.getNumber("ANGLE"));
            System.out.println(server.getNumber("POWER"));
            
//            if (server.getNumber("Locked") == 0){
//            lockedmiddle = false;
//            lockedtop = false;
//            }
//            if (server.getNumber("Locked") == 1){
//                lockedmiddle = true;
//                lockedtop = false;
//            }
//            if (server.getNumber("Locked") == 2){
//                lockedmiddle = false;
//                lockedtop = true;
//            }
        
            kinectangle = server.getNumber("ANGLE");
          kinectDistance = server.getNumber("DISTANCE");
	}
	catch (TableKeyNotDefinedException ex)
	{
	}

			
    }
    public void autoshooting(){
        //where all the autonomous shooting code will go
    }
    public void teleshooting(){
        //where all operator control shooter code will go
        if (ThirdStick.getRawButton(1) == true ){
            //loader.set(1.0);
            if (override == true){
            shooter1.set(-((ThirdStick.getZ(GenericHID.Hand.kLeft))+1)/2);
            shooter2.set(-((RightStick.getZ(GenericHID.Hand.kLeft))+1)/2);
            }else{
                shooter1.set(-.579);
                shooter2.set(-.854);
            }
        }else{
            //loader.set(0.0);
            shooter1.set(0.0);
            shooter2.set(0.0);
        }
    }
    public void driving(){
//                    if (RightStick.getRawButton(3) == true){
//            drivemode = 0;
//            }
//            if (RightStick.getRawButton(2) == true){
//                drivemode = 1;
//                
//            }
//            
//            if (drivemode == 1) {
        if (LeftStick.getRawButton(1) == true){
            drive.tankDrive(-.75*(LeftStick.getY(GenericHID.Hand.kLeft)), -.75*(RightStick.getY(GenericHID.Hand.kLeft)));
        }else{
//                
                drive.tankDrive(-(LeftStick.getY(GenericHID.Hand.kLeft)), -(RightStick.getY(GenericHID.Hand.kLeft)));
        }
//            }else{
//            
//                
//            
//            drive.arcadeDrive(-(LeftStick.getY(GenericHID.Hand.kLeft)), (-(RightStick.getX(GenericHID.Hand.kLeft))));
//            }
    }
    public void tilt(){
    if (ThirdStick.getRawButton(3)== true){ 

        tilt1.set(-(ThirdStick.getY(GenericHID.Hand.kLeft)));
    tilt2.set(-(ThirdStick.getY(GenericHID.Hand.kLeft)));
    
    }else{
        tilt1.set(0.0);
        tilt2.set(0.0);
    }
    
    }

    public void loading(){
        // NOT USED (I BELIEVE)
//    if (loaderlimit.get() == true){
//        loader.set(0.0);
//    }else{
//        loader.set(loadspeed);
//    }
             
        autotimes = 0;
        
        Servo2.setAngle(70);
        servo.set(175);
       // servo.setAngle(dist);
        update();
        tracking();
//       while ((kinectDistance > 20.25) || (kinectDistance < 19.75)){
//        autoadjust();
//        update();
//    }
        drive.tankDrive(0.0, 0.0);
        System.out.println(kinectDistance);
    shooter1.set(-.579);
    shooter2.set(-.854);
    Timer.delay(3);
    autotrigger = true;
//    while(autotimes < 3){
//
//        
//            loaderstat = 1;
//        
//        if (loaderstat == 1){
//            while (loaderlimit.get() != true){
//            loader.set(-1.0);
//            }
//            if (loaderlimit.get() == true){
//                loaderstat = 2;
//            }
//        }
//        if(loaderstat == 2){
//            while (loaderlimit.get() != false){
//                loader.set(-1.0);
//            }
//            if (loaderlimit.get() == false){
//                loader.set(0.0);
//                loaderstat = 0;
//                
//
//            }
//        }
//
//        Timer.delay(1.5);
//        Servo2.setAngle(6);
//        Timer.delay(1);
//        Servo2.setAngle(70);
//        servo.setAngle(175);
//        Timer.delay(1);
//        servo.setAngle(70);
//        ++autotimes;
//        autotrigger = true;
    
//    }

      
      shooter1.set(0.0);
      shooter2.set(0.0);
    loader.set(0.0);
    
}
    public void autonomous() {
        target = 1;  // We are going towards target one 
        autotimes = 0; // initatizing autotimes back to 0.
        servo.setAngle(70);//catch 1
        Servo2.setAngle(70);// catch 2
        tracking(); // calling upon our tracking method
        shooter1.set(-.579); // sets the lower speed of the lower wheel
        shooter2.set(-1.0); // sets the speed of the upper wheel
        tilt1.set(1.0);// running the elevator down
        tilt2.set(1.0);// running the elevator down
        Timer.delay(9.5); // run the timers
        tilt1.set(0.0); // stops the elevator
        tilt2.set(0.0); // stops the elevator
//        while ((kinectangle > 1) || (kinectangle < -1 )){
//        autoalign();
//    }
        loaderstat = 1; // loader status to 1   

    
        while (autotimes < 3){ // till you have all the frisbee have been shot
            
            if (loaderstat == 1){ // loader status 1 sets the green arm to move
            loader.set(-1.0);
            }
             if (loaderlimit.get() == true){ // getting switch value
            loaderstat = 2; // set it to 2 
            }
            if(loaderstat == 2){ // if 2
                Timer.delay(.1); // needed delay (uncertain why)
                if (loaderlimit.get() == false){ // once the hand is off the switch
                    loader.set(0.0);// stop hand
                    loaderstat = 0; // set the loader status to 0
                    
                    Servo2.setAngle(6); // sets the bottom server to 6
                    
                    
                    if (autotimes == 1){// if the second time running through then
                    servo.setAngle(175); // set the top servo to down
                    }
                }
            }
            if (loaderstat == 0){// 
            Timer.delay(1.5);// time for frisbee to drop
            loaderstat = 1;// sets the loader stat to 1
            
            
            autotimes++;// autotimes is increased
            }
       }
        if (autotimes == 3){// when done
            shooter1.set(0.0);// kill the shooter wheel
            shooter2.set(0.0);// kill the shooter wheel
            loaderstat = 0;// loader stat to zero
        }
}
    public void operatorControl() {
        //stuff that goes before we start Operator Control
        target = 1; // tells which target we are headed towards          
        autodistance1 = 20.5; // sweet spot margin
        autodistance2 = 19.5; // sweet spot margin
        shooter1.set(0.0); // setting shooter off just in case of emergencys
        shooter2.set(0.0); // setting shooter off just in case of emergencys
        while (true && isOperatorControl() && isEnabled()) {
            teleloader();// calls upon the teleloader method
            tracking();// calls upon the kinect info
            update();// update the the smart dashboard
            tilt();// calls upon the tilt method
            if (LeftStick.getRawButton(4) == true){// if pressed button 4
                target = 1; // switch to target 1 
            }
            if (LeftStick.getRawButton(5)== true){// if pressed button 5
                target = 2; // switch to target 2
            }
              if (ThirdStick.getRawButton(1) == true ){// if the shooter trigger is pressed
            //loader.set(1.0);
          
                shooter1.set(-.579);// set the lower wheel 
                shooter2.set(-1.0);// set the upper wheel
            
        }else{  
            //loader.set(0.0);
            shooter1.set(0.0);// stop the wheels
            shooter2.set(0.0);
        }
            driving(); // calls upon the driving method     
           // drive.tankDrive(LeftStick, RightStick);
           if (LeftStick.getRawButton(8) == true){ // if button 8 is pressed
            autoalign();// run autoalign
           }
           
            if (ThirdStick.getRawButton(4)== true){ //set the servo 
                servo.setAngle(175);// open the servo
                
            }else{
                servo.setAngle(70); // close the servo
            }
            if ((RightStick.getRawButton(11) == true)){//
                override = true;
            }
            if (RightStick.getRawButton(10) == true){
                override = false;
            }
            if (ThirdStick.getRawButton(2) == true){
                Servo2.setAngle(6);// open the servo
            }else{
                Servo2.setAngle(70); // close the servo
             }
            }
       
//        if (ThirdStick.getRawButton(9) == true){//pyramind
//            if (pot.getVoltage() >= 3.82){
//                tilt1.set(1.0);
//                tilt2.set(1.0);
//        }
//            if (pot.getVoltage() <= 3.75){
//                tilt1.set(-1.0);
//                tilt2.set(-1.0);
//            }
//            if (((pot.getVoltage() > 3.75) & (pot.getVoltage() < 3.9))){
//                tilt1.set(0.0);
//                tilt2.set(0.0);
//            }
//    }
//                if (ThirdStick.getRawButton(10) == true){//feeder
//            if (pot.getVoltage() >= 3.9){
//                tilt1.set(1.0);
//                tilt2.set(1.0);
//        }
//            if (pot.getVoltage() <= 3.8){
//                tilt1.set(-1.0);
//                tilt2.set(-1.0);
//            }
//            if (((pot.getVoltage() > 3.8) & (pot.getVoltage() < 3.9))){
//                tilt1.set(0.0);
//                tilt2.set(0.0);
//            }
//    }
//     if (ThirdStick.getRawButton(11) == true){
//            if (pot.getVoltage() > 3.2){
//                tilt1.set(1.0);
//                tilt2.set(1.0);
//        }
//            if (pot.getVoltage() < 3){
//                tilt1.set(-1.0);
//                tilt2.set(-1.0);
//            }
//            if (((pot.getVoltage() >= 3.0) & (pot.getVoltage() <= 3.2))){
//                tilt1.set(0.0);
//                tilt2.set(0.0);
//            }
    }
           }
    